function [xnew, lambda, pos,mu] = mymonqp(H,c,A,b,C)
% function [xnew, lambda, pos] = monqp(H,c,A,b,C,l,verbose,X,ps,xinit)
%
% min 1/2  x' H x - c' x
%  x
% contrainte   A' x = b
%
% et         0 <= x_i  <= C_i
%
% Cr?dits : J.P. Yvon (INSA de Rennes) pour l'algorithme
%           O. Bodard (Clermont Ferrand) pour le debugage on line
%
%  S CANU - scanu@insa-rouen.fr
%  Mai 2001
%--------------------------------------------------------------------------
%                                verifications
%--------------------------------------------------------------------------
[n,d] = size(H);
[nl,nc] = size(c);
[nlc,ncc] = size(A);
[nlb,ncb] = size(b);
if d ~= n
    error('H must be a squre matrix n by n');
end
if nl ~= n
    error('H and c must have the same number of row');
end

if nlc ~= n
    error('H and A must have the same number of row');
end
if nc ~= 1
    error('c must be a row vector');
end
if ncb ~= 1
    error('b must be a row vector');
end
if ncc ~= nlb
    error('A'' and b  must have the same number of row');
end
l = 1e-8;
verbose = 0;
C=ones(nl,1)*C;  % vectorizing the UpperBound Constraint
xinit=[];

fid = 1; %default value, curent matlab window
%--------------------------------------------------------------------------


%--------------------------------------------------------------------------
%                       I N I T I A L I S A T I O N
%--------------------------------------------------------------------------

OO = zeros(ncc);
H = H+l*eye(length(H)); % preconditionning

xnew = -1*ones(size(C));ness = 0;
ind=1;



if isempty(xinit)
    
    while (( sum (xnew < 0)) >0 | ( sum(xnew >C(ind)) > 0))  & ness < 100   %% 03/01/2002
        ind = randperm(n);
        ind = sort(ind(1:ncc)');
        aux=[H(ind,ind) A(ind,:);A(ind,:)' OO];
        aux= aux+l*eye(size(aux));
        if rcond(aux)>1e-12
            newsol = aux\[c(ind) ; b];
            xnew = newsol(1:length(ind));
            ness = ness+1;
        else
            ness=101;
            break;
        end;
    end;
    %keyboard
    if ness < 100
        x = xnew;
        lambda = newsol(length(ind)+1:length(ind)+ncc);
    else
        % Brute Force Initialisation
        ind = [1];
        x = C(ind)/2.*ones(size(ind,1),1);
        lambda=ones(ncc,1);
        
        %
        %         ind = [1;nl];
        %         x = C(ind)/2.*ones(size(ind,1),1);
        
        
    end
    indsuptot = [];
else  % start with a predefined x
    
    
    indsuptot=find(xinit==C);
    indsup=indsuptot;
    ind=find(xinit>0 & xinit ~=C) ;
    x = xinit(ind);
    lambda=ones(ncc,1);
    
end;



% Modification for QP without equality constraints
if sum(A==0)
    ncc=0;
end;


[U,testchol] = chol(H); % Test definite positiveness of H
%--------------------------------------------------------------------------
%                            M A I N   L O O P
%--------------------------------------------------------------------------

Jold = 10000000000000000000;
%C = Jold; % for the cost function
if verbose ~= 0
    disp('      Cost     Delta Cost  #support  #up saturate');
    nbverbose = 0;
end

nbiter=0;
STOP = 0;
nbitermax=20*n;
while STOP ~= 1
    
    nbiter=nbiter+1;
    indd = zeros(n,1);indd(ind)=ind;nsup=length(ind);indd(indsuptot)=-1;
    if verbose ~= 0
        
        [J,yx] = cout(H,x,b,C,indd,c,A,b,lambda);
        
        nbverbose = nbverbose+1;
        if nbverbose == 20
            disp('      Cost     Delta Cost  #support  #up saturate');
            nbverbose = 0;
        end
        if Jold == 0
            fprintf(fid,'| %11.4e | %8.4f | %6.0f | %6.0f |\n',[J (Jold-J) nsup length(indsuptot)]);
        elseif  (Jold-J)>0
            fprintf(fid,'| %11.4e | %8.4f | %6.0f | %6.0f |\n',[J min((Jold-J)/abs(Jold),99.9999) nsup length(indsuptot)]);
        else
            fprintf(fid,'| %11.4e | %8.4f | %6.0f | %6.0f | bad mouve \n',[J max((Jold-J)/abs(Jold),-99.9999) nsup length(indsuptot)]);
            
        end
        Jold = J;
        
    end
    
    
    
    
    
    % nouvele resolution du syst?me
    
    % newsol = [H(ind,ind)+l*eye(length(ind)) A(ind,:);A(ind,:)' OO]\[c(ind) ; b];
    % xnew = newsol(1:length(ind));
    % lambda = newsol(length(ind)+1:length(newsol));
    
    ce = c(ind);
    be = b;
    
    %     if (isempty(indsuptot)==0) % if NOT empty
    %         keyboard
    %         %     ce= ce - C*(sum(H(ind,indsuptot),2)+sum(H(indsuptot,ind),1)');%    min     x'Hx + c'x
    %         ce= ce - C*(sum(H(ind,indsuptot),2));%                              min     x'Hx + c'x
    %         be= be - C*sum(A(indsuptot,:),1)';                            %    with    Ax=b
    %     end
    
    
    %03/01/2003
    if (isempty(indsuptot)==0) % if NOT empty
        
        %
        %  on remplace tout les x qui sont indsuptot par C
        %
        
        Cmat= ones(length(ind),1)*(C(indsuptot)');
        if size(ce)~= size(sum( Cmat.*H(ind,indsuptot),2))
            keyboard;
        end;
        ce= ce - sum( Cmat.*H(ind,indsuptot),2);%                              min     x'Hx + c'x
        Cmat= C(indsuptot)*ones(1,size(A,2));
        be= be - sum(Cmat.*A(indsuptot,:),1)';                            %    with    Ax=b
        
    end
    
    
    
    %  keyboard
    % auxH=H(ind,ind);
    % [U,testchol] = chol(auxH); %
    At=A(ind,:)';
    Ae=A(ind,:);
    
    
    if testchol==0
        auxH=H(ind,ind);
        [U,testchol] = chol(auxH); % It would be better to use an updated choleski
        %------------------------------------------------------------------
        %         if length(ind)>5
        %             keyboard
        %         if firstchol
        %             Ub=chol(auxH)';
        %             firstchol=0;
        %         else
        %
        %             if directioncholupdate==1
        %                 Ubr = updatechol(Ub,indexcholupdate-1,H(varcholupdate,:),directioncholupdate);
        %             else
        %                 Ubr = updatechol(Ub,indexcholupdate,[],directioncholupdate);
        %             end;
        %
        %             max(max(abs(Ub-U)))
        %         end;
        %     end
        %------------------------------------------------------------------
        
        M = At*(U\(U'\Ae));
        d = U\(U'\ce);
        d = (At*d - be);   % second membre  (homage au gars OlgaZZZZZZ qui nous a bien aid?)
        % On appelle le gc fabuleux de mister OlgaZZZ Hoduc
        % lambdastart = rand([m,1]);
        % [lambda] = gradconj(lambdastart,M*M,M*d,MaxIterZZZ,tol,1000);
        if rcond(M)<l
            M=M+l*eye(size(M));
        end;
        lambda = M\d;
        % On reconstitue le vecteur en r?solvant le syst?me lin?aire Hx = c-Alambda
        % size(lambda)
        % size(Ae)
        %      keyboard
        %        xnew = U\(Ut\(ce-Ae*lambda));
        xnew=auxH\(ce-Ae*lambda);
    else
        
        % if non definite positive;
        auxH=H(ind,ind);
        
        auxM=At*(auxH\Ae);
        M=auxM'*auxM;
        d=auxH\ce;
        d=At*d-be;
        if rcond(M)<l
            M=M+l*eye(size(M));
        end;
        lambda=M\d;
        xnew=auxH\(ce-Ae*lambda);
    end;
    
    %-------------------------------------------------------------------------------------------------------    
    [minxnew minpos]=min(xnew);
    
    if (sum(xnew< 0) > 0 | sum(xnew > C(ind)) > 0)   &  length(ind) > ncc  %  03/01/2002 AR
        %  cette condition est utile  pour le
        % semi param
        % on projette dans le domaine admissible et on sature la contrainte associ?e
        %-------------------------------------------------------------------------
        d = (xnew - x)+l;                % projection direction
        indad = find(xnew < 0);
        indsup = find(xnew > C(ind) );                      %% 03/01/2002
        [tI indmin] = min(-x(indad)./d(indad));
        [tS indS] = min((C(ind(indsup))-x(indsup))./d(indsup)); % pas de descente
        if isempty(tI) , tI = tS + 1; end;
        if isempty(tS) , tS = tI + 1; end;
        t = min(tI,tS);
        
        x = x + t*d;                 % projection into the admissible set
        
        if t == tI
            
            varcholupdate=ind(indad(indmin));
            indexcholupdate=indad(indmin);
            directioncholupdate=-1; % remove
            
            ind(indad(indmin)) = [];            % the associate variable is set to 0
            x(indad(indmin)) = [];              % the associate variable is set to 0
            
            
            
        else
            indexcholupdate=indsup(indS);
            varcholupdate=ind(indsup(indS));
            directioncholupdate=-1; % remove
            
            indsuptot = [indsuptot ; ind(indsup(indS))];
            ind(indsup(indS))= [];
            x(indsup(indS))= [];
            
            
            
        end
        
    else
        xt = zeros(n,1);           % keyboard;
        xt(ind) = xnew;              % keyboard;
        xt(indsuptot) = C(indsuptot); indold=ind;             % keyboard;                                     %% 03/01/2002
        
        mu = H*xt - c + A*lambda;    % calcul des multiplicateurs de lagrange associ?es aux contraintes
        
        indsat = 1:n;                           % on ne regarde que les contraintes satur?es
        indsat([ind ; indsuptot]) = [];
        
        [mm mpos]=min(mu(indsat));
        [mmS mposS]=min(-mu(indsuptot));  %keyboard
        
        if ((~isempty(mm) & (mm < -sqrt(eps)))  | (~isempty(mmS) & (mmS <  -sqrt(eps)))) & nbiter< nbitermax
            if isempty(indsuptot) | (mm < mmS)             % il faut rajouter une variable
                ind = sort([ind ; indsat(mpos)]);        % on elimine une contrainte de type x=0
                x = xt(ind);
                indexcholupdate=find(ind==indsat(mpos));
                varcholupdate=indsat(mpos);
                directioncholupdate=1; % remove
            else
                ind = sort([ind ; indsuptot(mposS)]);  % on elimine la contrainte sup si necessaire
                x = xt(ind);                           % on elimine une contrainte de type x=C
                indexcholupdate=find(ind==indsuptot(mposS));
                varcholupdate=indsuptot(mposS);
                indsuptot(mposS)=[];
                directioncholupdate=1; % remove
            end
        else           
            STOP = 1;
            pos=sort([ind ; indsuptot]);
            xt = zeros(n,1);
            xt(ind) = xnew;
            xt(indsuptot) = C(indsuptot);
            indout = 1:n;
            indout(pos)=[];
            xnew = xt;
            xnew(indout)=[];          
        end
        
    end
end
